#!/usr/bin/env python
import roslib; roslib.load_manifest('Eyebox2')
import sys
import rospy

from telnetlib import Telnet
from Eyebox2.msg import *

class Person:
    def __init__(self):
#        self.face_active = False
#        self.face_time = None
        self.face_position = ((0, 0), (0, 0))

        self.eyes_detected = False
        self.eyes_looking = False        
#        self.eyes_time = None
        self.eye_positions = ((0, 0), (0, 0))

    def __repr__(self):
        return 'repr'

    def __str__(self):
        return str(self.face_position) + str(self.eye_positions)

class Eyebox:
    def __init__(self, host):
        self.eyebox_pub = rospy.Publisher("eyebox_message",eye_box_messages)
        self.connection = Telnet(host, 5015)
        #self.test_file = open('test_eyebox_data.txt', 'r')
       
        self.people = {}

    def __del__(self):
        self.connection.close()

    def get_message(self):
        # Wait for the next message
        message = self.connection.read_until('\n')
#        message = self.test_file.readline()
        self.parse_message(message)
        

    # This function parses the message type and routes to the
    # corresponding helper function
    def parse_message(self, message):
        numbers = message.split()
        if numbers[0] == '1' or numbers[0] == '2':
            self.parse_position_message(numbers)
        else:
            self.parse_detection_message(numbers)

    # further routes to the appropriate helper function based on
    # the message type
    def parse_position_message(self, numbers):

        if numbers[0] == '1':  # Faces
            self.parse_face_position(numbers)
        elif numbers[0] == '2':  # Eyes
            self.parse_eye_position(numbers)
        else:
            print 'Unknown Message', numbers

        print numbers

    #this is the parsing and action function for message type 1
    #'Face Position'. This function should walk through each
    #reported face position and update the face position of
    #the appropriate person in the people dicitonary
    def parse_face_position(self,numbers):
        i = 2
        while i < len(numbers)-3:
            personID = numbers[i]
            if personID in self.people:
                self.people[personID].face_position = ((int(numbers[i+1]),int(numbers[i+2])),(int(numbers[i+3]),int(numbers[i+4])))
            i += 5            
        print "face position"


    #this is the parsing and action function for message type 2
    #'Eye Position'. This function should walk through each
    #reported eye positions and update the eye position of
    #the appropriate person in the people dicitonary
    def parse_eye_position(self,numbers):
        i = 2
        while i < len(numbers)-3:
            personID = numbers[i]
            if personID in self.people:
                self.people[personID].eye_positions = ((int(numbers[i+3]),int(numbers[i+4])),(int(numbers[i+7]),int(numbers[i+8])))
            i += 9            
        print "eye position"



    # further routes to the appropriate helper function based on
    # the message type
    def parse_detection_message(self, numbers):

        if numbers[0] == '5':
            self.parse_eye_detection(numbers)
        elif numbers[0] == '8':
            self.parse_face_detection(numbers)
        elif numbers[0] == '9':
            self.parse_eyes_looking(numbers)
        else:
            print 'Unknown Message', numbers

        print numbers

    #This function updates the state of eyes_looking and eyes_detected
    #for the person specified by the given personID
    def parse_eye_detection(self,numbers):
        personID = numbers[1]
        if personID in self.people:
            if numbers[2] == '0':
                self.people[personID].eyes_detected = False
                self.people[personID].eyes_looking = False
            elif numbers[2] == '1':
                self.people[personID].eyes_detected = True
                self.people[personID].eyes_looking = False
            elif numbers[2] == '2':
                self.people[personID].eyes_detected = True
                self.people[personID].eyes_looking = True
            else:
                print "Unknown Eye Detection Message"
                
            print "eye detected", numbers

    #This function will add or delete the appropriate person
    #from the people dictionary
    def parse_face_detection(self,numbers):
        personID = numbers[1]
        if numbers[2] == '0':
            self.del_person(personID)
        elif numbers[2] == '1':
            self.add_person(personID)
        else:
            print "Unknown Eye Detection Message"

        print "face detected", numbers
        
    def parse_eyes_looking(self,numbers):
        personID = numbers[1]
        if personID in self.people:
            if numbers[2] == '0':
                self.people[personID].eyes_detected = False
                self.people[personID].eyes_looking = False
            elif numbers[2] == '1':
                self.people[personID].eyes_detected = True
                self.people[personID].eyes_looking = True
            else:
                print "Unknown Eye Detection Message"

            print 'eyes looking',numbers

    #This function takes in a personID
    #If that ID is not already in the dictionary of people, add a new
    #person corresponding to that ID.
    def add_person(self, personID):
        if personID not in self.people:
            self.people[personID] = Person()

    #This function takes in a personID and deletes that person from the dictionary
    def del_person(self,personID):
        if personID in self.people:
            del self.people[personID]

    #This function constructs the eye_box_messages from the data received from eyebox2 and 
    #publish it to the "eye_box_messages" topic
    def pub_eyebox_message(self):
        IDs = []        
        facePos = []        
        eyePos = []
        eyeDetect = []
        eyeLooking = []
        temp_message = eye_box_messages()

        for ID, person in self.people.items():
            
            for a in list(person.face_position):
                facePos.extend(list(a))
            
            for a in list(person.eye_positions):
                eyePos.extend(list(a))

            IDs.append(ID)
            eyeDetect.append(person.eyes_detected)
            eyeLooking.append(person.eyes_looking)
            
        temp_message.personID = IDs
        temp_message.face_position = facePos
        temp_message.eye_positions = eyePos
        temp_message.eyes_detected = eyeDetect
        temp_message.eyes_looking = eyeLooking

            
        self.eyebox_pub.publish(temp_message)

            
    
if __name__ == '__main__':
    rospy.init_node('eyebox', anonymous=True)
    try:
   
        #This creates a new instance of the Eyebox class with the 
        #specified host address.
        eyebox = Eyebox('192.168.1.128')

#        for i in range(10):
        while 1:
            eyebox.get_message()
            eyebox.pub_eyebox_message()

            for id,p in eyebox.people.items():
                print id, p
            print
    except KeyboardInterrupt:
        print "Shutting down"  
